/***************************************************************************
 创建者: 华磊
 开始时间: 2019.2.25
 copyright: (C) 华友高科
 修改说明: (每次有修改就添加一条，带有 修改人，修改时间，修改描述)
 example (1) hualei 2020.5.8 类的具体实现代码编写
 ***************************************************************************/
#include "qrcodesensoradapter.h"
#ifdef D_USE_OPENCV
#include "qrcodedetect.h"
#endif
#include "domparser.h"
#include <QDebug>
#define IF_DEBUG 1

QrcodeSensorAdapter::QrcodeSensorAdapter(QString configFileIn, QObject *parent) : QObject(parent)
{
    isInitialOkStatus=false;
    debugInfo.minConsumed=100000000000;
    debugInfo.currentConsumed=0;
    debugInfo.maxConsumed=0;
    if(1==initialConfigFile(configFileIn))
    {
        if(true==isUseSensor)
        {
            #ifdef D_USE_OPENCV
            qrcodeDetect=new QrcodeDetect(detectPara.allowMinArea,detectPara.allowMaxArea,detectPara.allowMinLength,
                                          detectPara.allowMaxLength,detectPara.colorThrehold);
            isInitialOkStatus=true;
            timer=new QTimer();
            connect( timer, SIGNAL(timeout()), this, SLOT(timeOut_slot()) );
            timer->start( 120 );
            #endif
        }
        else
        {
            isInitialOkStatus=true;
        }

    }
    else
    {

    }
}

bool QrcodeSensorAdapter::isInitialOk()
{
    return isInitialOkStatus;
}

bool QrcodeSensorAdapter::isUseThisModule()
{
    return isUseSensor;
}

int QrcodeSensorAdapter::getVehiclePosition(PositionInfo &positionOut)
{
    if(false==isInitialOkStatus)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return 0;
    }

    else if(false==debugInfo.isDeviceOpened)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -10;
    }
    else if(false==debugInfo.isImageAcquired)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -11;
    }
    else if(3>debugInfo.featureSqureCount)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -12;
    }
    else if(3<debugInfo.featureSqureCount)
    {
        positionOut.isUpdateSucess=false;
        positionOut.matchScore=0;
        positionOut.maxMatchError=-1;
        positionOut.position=positionFeedback;
        return -13;
    }

    positionOut.isUpdateSucess=true;
    positionOut.matchScore=100;
    positionOut.maxMatchError=0;
    positionOut.position=positionFeedback;
}

int QrcodeSensorAdapter::setCameraId(int cameraIdIn)
{
    sensorId=cameraIdIn;
    return 1;
}

int QrcodeSensorAdapter::showDebugWindow(int id)
{
    #ifdef D_USE_OPENCV
    return qrcodeDetect->showDebugWindow(id);
    #endif
}

int QrcodeSensorAdapter::getQrcodeSensorDebugInfo(QrcodeDebugInfo &debugInfoOut)
{
    #ifdef D_USE_OPENCV
    debugInfo.positionFeedback=positionFeedback;
    debugInfo.sensorId=sensorId;
    debugInfoOut=debugInfo;
    #endif
    return 1;
}

int QrcodeSensorAdapter::setDetectParameter(QrcodeDetectParameter parameterIn)
{
    #ifdef D_USE_OPENCV
    return qrcodeDetect->setDetectParameter(parameterIn.allowMinArea,parameterIn.allowMaxArea
                               ,parameterIn.allowMinLength,parameterIn.allowMaxLength,parameterIn.colorThrehold);
    #endif
}

void QrcodeSensorAdapter::timeOut_slot()
{
    updatePosition(sensorId,positionFeedback,debugInfo.isDeviceOpened,debugInfo.isImageAcquired,debugInfo.featureSqureCount);
}

int QrcodeSensorAdapter::initialConfigFile(QString configFileName)
{
    qDebug() << "QrcodeSensorAdapter::initialConfigFile ==== 初始化配置文件：" << configFileName;

    DomParser domparser;
    bool ok = domparser.openXml(configFileName, QIODevice::ReadOnly);
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile ==== 配置文件打开失败：" << configFileName;
        }
        return -1;
    }

    QDomNode domNode = domparser.findSubNode(domparser.getRootDomElement(), "isUseSensor", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, isUseSensor);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "sensorId", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, sensorId);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_x", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_x);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_y", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_y);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_z", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_z);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_yaw", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_yaw);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_pitch", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_pitch);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "pose_roll", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, pose_roll);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "x_ratio", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, x_ratio);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "y_ratio", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, y_ratio);

    domNode = domparser.findSubNode(domparser.getRootDomElement(), "r_ratio", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, r_ratio);

    
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "allowMinArea", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, detectPara.allowMinArea);
    
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "allowMaxArea", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, detectPara.allowMaxArea);
    
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "allowMinLength", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, detectPara.allowMinLength);
    
    domNode = domparser.findSubNode(domparser.getRootDomElement(), "allowMaxLength", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, detectPara.allowMaxLength);


    domNode = domparser.findSubNode(domparser.getRootDomElement(), "colorThrehold", ok );
    if( !ok )
    {
        if(IF_DEBUG)
        {
            qDebug() << "QrcodeSensorAdapter::initialConfigFile failed" << configFileName;
        }
            return -1;
    }
    domparser.readXml(domNode, detectPara.colorThrehold);

    domparser.closeXml();
    return 1;
}

int QrcodeSensorAdapter::updatePosition(int cameraIdIn, VehiclePosition &posOut,
                                        bool &isDeviceOpenedOut, bool &isImageAcquiredOut, int &featureSqureCountOut)
{
    #ifdef D_USE_OPENCV
    //时间分析
    struct timespec startTime,endTime;
    clock_gettime(CLOCK_MONOTONIC, &startTime);

    double xOut, yOut, rOut;
    int resultKey=qrcodeDetect->detectQrcode(cameraIdIn,xOut, yOut, rOut,featureSqureCountOut);
    if(-100==resultKey)
    {
        isDeviceOpenedOut=false;
        return resultKey;
    }
    else if(0==resultKey)
    {
        isImageAcquiredOut=false;
        return resultKey;
    }
    else if(-1==resultKey)
    {
        isDeviceOpenedOut=true;
        isImageAcquiredOut=true;
//        return resultKey;
    }
    else if(1==resultKey)
    {
        //todo ,需要用逆矩阵，求解，以二维码为坐标系去确定车的位置（不能以车的坐标系确定二维码的位置然后控制）
        //摄像头放置前方时，离地面有个倾斜角，这个倾斜角约３０度，导致不能近视垂直去处理，位置和角度耦合较大。
        //差分驱动轮，只适合摄像头几乎完全垂直地面安装。麦克纳姆轮可能适合带有倾斜角。
        positionFeedback.x=xOut*x_ratio-pose_x;
        positionFeedback.y=yOut*y_ratio-pose_y;
        positionFeedback.rotate=rOut*r_ratio-pose_yaw;
        isDeviceOpenedOut=true;
        isImageAcquiredOut=true;
    }

    clock_gettime(CLOCK_MONOTONIC, &endTime);
    double tmpNs=1000000000*(endTime.tv_sec-startTime.tv_sec)+endTime.tv_nsec-startTime.tv_nsec;//ns
    debugInfo.currentConsumed=tmpNs/1000000.0;

    if(debugInfo.currentConsumed<debugInfo.minConsumed)
    {
        debugInfo.minConsumed=debugInfo.currentConsumed;
    }
    if(debugInfo.currentConsumed>debugInfo.maxConsumed)
    {
        debugInfo.maxConsumed=debugInfo.currentConsumed;
    }


    return resultKey;
    #endif
    return 1;

}
